#include "remote_control_core.h"

RemoteControl::RemoteControl():nh_(""), private_nh_("~"){
    private_nh_.param<std::string>("input_costmapCompress_topic_", input_costmapCompress_topic_, "");
    private_nh_.param<std::string>("input_gnssStatus_topic", input_gnssStatus_topic, "");
    private_nh_.param<std::string>("input_carInfo_topic_", input_carInfo_topic_, "");
    private_nh_.param<std::string>("output_vehicleToRemote_topic_", output_vehicleToRemote_topic_, "");
}

RemoteControl::~RemoteControl(){}

void RemoteControl::costmapCompress_callback(const costmap_compress::CompressedOccupancyGrid::ConstPtr& msg){
    costmap_compress_msg_ = *msg;
}
void RemoteControl::gnssStatus_callback(const robocoachMsg::GnssStatus::ConstPtr& msg){
    gnss_status_msg_ = *msg;
}
void RemoteControl::carInfo_callback(const robocoachMsg::CarInfo::ConstPtr& msg){
    car_info_msg_ = *msg;
}

void RemoteControl::process(const ros::TimerEvent& e){
    remote_control::vehicleToRemote vehicleToRemote_msg_;
    // 系统时间
    ros::WallTime start = ros::WallTime::now();
    vehicleToRemote_msg_.realTime = start.toSec()*1000.0;
    // 车牌
    vehicleToRemote_msg_.licensePlate = "京A12345学";

    // 车辆信息
    // 车辆位置姿态
    vehicleToRemote_msg_.east = gnss_status_msg_.east;
    vehicleToRemote_msg_.north = gnss_status_msg_.north;
    vehicleToRemote_msg_.yaw = gnss_status_msg_.yaw;
    // 局部代价地图
    vehicleToRemote_msg_.resolution = costmap_compress_msg_.info.resolution;
    vehicleToRemote_msg_.width = costmap_compress_msg_.info.width;
    vehicleToRemote_msg_.height = costmap_compress_msg_.info.height;
    vehicleToRemote_msg_.x_origin = costmap_compress_msg_.info.origin.position.x;
    vehicleToRemote_msg_.y_origin = costmap_compress_msg_.info.origin.position.y;
    // 判断vehicleToRemote_msg_.x_origin不为空，再计算四角的坐标
    if(vehicleToRemote_msg_.x_origin!=0)
    {
        // 四角计算
        std::vector<std::pair<double, double>> coordinates_fourCorners0 =  fourCornersAnalysis(vehicleToRemote_msg_);
        for (const auto& coord : coordinates_fourCorners0) {
            remote_control::Point2D point;
            point.x = coord.first;
            point.y = coord.second;
            vehicleToRemote_msg_.four_corners.push_back(point);
        }
        // 打印vehicleToRemote_msg_.four_corners的长度
        //ROS_INFO("vehicleToRemote_msg_.four_corners size: %zu", vehicleToRemote_msg_.four_corners.size());
        // 打印vehicleToRemote_msg_.four_corners的内容
        // for (const auto& corner : vehicleToRemote_msg_.four_corners) {
        //     ROS_INFO("Corner: (x: %.2f, y: %.2f)", corner.x, corner.y);
        // }
        // 障碍点计算
        // 打印costmap_compress_msg_.compressed_data长度
        //ROS_INFO("costmap_compress_msg_.compressed_data size: %zu", costmap_compress_msg_.compressed_data.size());
        std::vector<std::pair<double, double>> coordinates_obstacle1 =  obstacleAnalysis(vehicleToRemote_msg_, costmap_compress_msg_.compressed_data);
        for (const auto& coord : coordinates_obstacle1) {
            remote_control::Point2D point;
            point.x = coord.first;
            point.y = coord.second;
            vehicleToRemote_msg_.obstacles.push_back(point);
        }
        // 打印vehicleToRemote_msg_.obstacles的长度
        ROS_INFO("vehicleToRemote_msg_.obstacles size: %zu", vehicleToRemote_msg_.obstacles.size());
        obstacle_maxnum = std::max(obstacle_maxnum, static_cast<int>(vehicleToRemote_msg_.obstacles.size()));
        ROS_INFO("obstacle_maxnum: %d", obstacle_maxnum);
        // 打印vehicleToRemote_msg_.obstacles的内容
        // for (const auto& obstacle : vehicleToRemote_msg_.obstacles) {
        //     ROS_INFO("Obstacle: (x: %.2f, y: %.2f)", obstacle.x, obstacle.y);
        // }

    }
    

    
    
   
    // // 发布remoteControl_pub_
    remoteControl_pub_.publish(vehicleToRemote_msg_);
    
    


    // data analysis test
    // 车辆位置姿态
    // vehicleToRemote_msg_.east = 10;
    // vehicleToRemote_msg_.north = 10;
    // vehicleToRemote_msg_.yaw = 90;
    // // 局部代价地图
    // vehicleToRemote_msg_.resolution = 0.1;
    // vehicleToRemote_msg_.width = 100;
    // vehicleToRemote_msg_.height = 200;
    // vehicleToRemote_msg_.x_origin = -1.5;
    // vehicleToRemote_msg_.y_origin = -10;

    //outfile_start_count++;
    if(vehicleToRemote_msg_.x_origin!=0 && outfile_start_count==50)
    {
        ROS_INFO("fenxi baocun ");
        //记录原始数据
        std::ofstream outfile;
        outfile.open("/home/robot/Desktop/mqtt_test/mqtt_test_hunter/documents/vehicleToRemote_msg.txt", std::ios::app);  // 追加模式写入
        if (outfile.is_open()) {
            outfile << vehicleToRemote_msg_ << std::endl;
            outfile.close();
            ROS_INFO(" source Data saved to vehicleToRemote_msg.txt");
        } else {
            ROS_ERROR("Failed to open vehicleToRemote_msg!");
        }
        // 障碍点计算
        std::vector<std::pair<double, double>> coordinates_obstacle0 =  obstacleAnalysis(vehicleToRemote_msg_, costmap_compress_msg_.compressed_data);
        std::ofstream outfile2;
        outfile2.open("/home/robot/Desktop/mqtt_test/mqtt_test_hunter/documents/coordinates_obstacle0.txt", std::ios::app);  // 追加模式写入
        if (outfile2.is_open()) {
            for (const auto& coord : coordinates_obstacle0) {
                outfile2 << coord.first << " " << coord.second << std::endl; // 每行存储一对坐标
            }
            outfile2.close();
            ROS_INFO(" coordinates_obstacle Data saved to coordinates_obstacle0.txt");
        } else {
            ROS_ERROR("Failed to open file coordinates_obstacle0!");
        }
        //四角计算
        std::vector<std::pair<double, double>> coordinates_fourCorners0 =  fourCornersAnalysis(vehicleToRemote_msg_);
        std::ofstream outfile3;
        outfile3.open("/home/robot/Desktop/mqtt_test/mqtt_test_hunter/documents/coordinates_fourCorners0.txt", std::ios::app);  // 追加模式写入
        if (outfile3.is_open()) {
            for (const auto& coord : coordinates_fourCorners0) {
                outfile3 << coord.first << " " << coord.second << std::endl; // 每行存储一对坐标
            }
            outfile3.close();
            ROS_INFO(" coordinates_sijiao Data saved to coordinates_fourCorners0.txt");
        } else {
            ROS_ERROR("Failed to open file coordinates_fourCorners0!");
        }

    }
    
}

std::vector<std::pair<double, double>> RemoteControl::obstacleAnalysis(const remote_control::vehicleToRemote& msg, const std::vector<uint8_t>& compressed_data){
    // 基站坐标系车辆位置
    float vehicle_x = msg.north;
    float vehicle_y = -msg.east;

    // costmap地图原点
    double theta_1 = 0;
    if(abs(msg.x_origin-0.0)<=0.000001)
    {theta_1 = 0;}
    theta_1 = atan(msg.y_origin/msg.x_origin);  //弧度
    float vehicle_yaw = - M_PI * (msg.yaw/180.0); //逆时针为正
    double theta_2 = theta_1 + vehicle_yaw; //弧度
    float x_origin = vehicle_x - sqrt(msg.x_origin*msg.x_origin + msg.y_origin*msg.y_origin) * cos(theta_2);
    float y_origin = vehicle_y - sqrt(msg.x_origin*msg.x_origin + msg.y_origin*msg.y_origin) * sin(theta_2);
    
    // 障碍点计算
    std::vector<std::pair<double, double>> coordinates_obstacle;
    size_t idx = 0;
    int hang_num=0;
    int obstacle_num = 0;
    for (const auto& byte : compressed_data) {
        hang_num++;
        for (int i=0; i<8 && idx<compressed_data.size()*8; ++i) {
            bool bit = (byte >> i) & 0x01; // 低位优先解包
            if(bit)
            {
                obstacle_num++;
                int index = idx;
                int index_x = index % msg.width;
                int index_y = index / msg.width;
                // ROS_INFO("%d hang id  byte: %d",hang_num,byte);
                // ROS_INFO("index is: (index_x:%d, index_y:%d, index is :%d)", index_x, index_y, index);

                // 障碍中心点
                double theta_3 = atan(((index_y+0.5)*msg.resolution)/((index_x+0.5)*msg.resolution));
                float obstacle_distance = sqrt(pow((index_x+0.5)*msg.resolution, 2) + pow((index_y+0.5)*msg.resolution, 2));
                double theta_4 = theta_3 + vehicle_yaw;
                float obstacle_x = x_origin + obstacle_distance*cos(theta_4);
                float obstacle_y = y_origin + obstacle_distance*sin(theta_4);
                coordinates_obstacle.push_back({obstacle_x,obstacle_y});
            }
            idx++;
        }
    }
    // ROS_INFO("obstacle_num is : %d",obstacle_num);
    return coordinates_obstacle;

}

std::vector<std::pair<double, double>> RemoteControl::fourCornersAnalysis(const remote_control::vehicleToRemote& msg){
    // 基站坐标系车辆位置
    float vehicle_x = msg.north;
    float vehicle_y = -msg.east;

    // costmap地图原点
    double theta_1 = 0;
    if(abs(msg.x_origin-0.0)<=0.000001)
    {theta_1 = 0;}
    theta_1 = atan(msg.y_origin/msg.x_origin);  //弧度
    float vehicle_yaw = - M_PI * (msg.yaw/180.0); //逆时针为正
    double theta_2 = theta_1 + vehicle_yaw; //弧度
    float x_origin = vehicle_x - sqrt(msg.x_origin*msg.x_origin + msg.y_origin*msg.y_origin) * cos(theta_2);
    float y_origin = vehicle_y - sqrt(msg.x_origin*msg.x_origin + msg.y_origin*msg.y_origin) * sin(theta_2);
    

    //计算四角位置
    std::vector<std::pair<double, double>> coordinates_sijiao;
    coordinates_sijiao.push_back({x_origin,y_origin});// 0
    float sijiao_x = x_origin + msg.width*msg.resolution*cos(vehicle_yaw);
    float sijiao_y = y_origin + msg.width*msg.resolution*sin(vehicle_yaw);
    coordinates_sijiao.push_back({sijiao_x,sijiao_y});// 1
    float sijiao2_dis = sqrt(pow(msg.width*msg.resolution, 2) + pow(msg.height*msg.resolution, 2));
    sijiao_x = x_origin + sijiao2_dis*cos(atan(msg.height/msg.width)+vehicle_yaw);
    sijiao_y = y_origin + sijiao2_dis*sin(atan(msg.height/msg.width)+vehicle_yaw);
    coordinates_sijiao.push_back({sijiao_x,sijiao_y});// 2
    sijiao_x = x_origin + msg.height*msg.resolution*cos(M_PI/2+vehicle_yaw);
    sijiao_y = y_origin + msg.height*msg.resolution*sin(M_PI/2+vehicle_yaw);
    coordinates_sijiao.push_back({sijiao_x,sijiao_y});// 3

    return coordinates_sijiao;
}



// void RemoteControl::obstacleAnalysis(const remote_control::vehicleToRemote& msg){
//     // 创建 Marker 消息
//     visualization_msgs::Marker points;
//     points.header.frame_id = "map"; // 设置参考坐标系
//     points.header.stamp = ros::Time::now(); // 设置时间戳
//     points.ns = "points"; // 设置命名空间
//     points.action = visualization_msgs::Marker::ADD; // 设置动作（添加）
//     points.pose.orientation.w = 1.0; // 设置姿态（单位四元数）
//     points.id = 0; // 设置 ID
//     points.type = visualization_msgs::Marker::POINTS; // 设置类型（点）

//     // 设置点的大小
//     points.scale.x = 1;
//     points.scale.y = 1;

//     geometry_msgs::Point point0;
//     point0.x = 0.0;
//     point0.y = 0.0;
//     point0.z = 0.0;
//     points.points.push_back(point0);
//     std_msgs::ColorRGBA color1;
//     color1.r = 1.0f; // 红色
//     color1.g = 0.0f;
//     color1.b = 0.0f;
//     color1.a = 1.0f;
//     points.colors.push_back(color1);

//     // 基站坐标系车辆位置
//     float vehicle_x = msg.north;
//     float vehicle_y = -msg.east;
//     ROS_INFO("vehicle is in world: (x:%f, y:%f)", vehicle_x, vehicle_y);
//     geometry_msgs::Point point1;
//     point1.x = vehicle_x;
//     point1.y = vehicle_y;
//     point1.z = 0.0;
//     points.points.push_back(point1);
//     std_msgs::ColorRGBA color2;
//     color2.r = 0.0f; // 绿色
//     color2.g = 1.0f;
//     color2.b = 0.0f;
//     color2.a = 1.0f;
//     points.colors.push_back(color2);


//     // costmap地图原点
//     double theta_1 = 0;
//     if(abs(msg.x_origin-0.0)<=0.000001)
//     {theta_1 = 0;}
//     theta_1 = atan(msg.y_origin/msg.x_origin);  //弧度
//     ROS_INFO("costmap_origin is in base_link: (x:%f, y:%f)", msg.x_origin, msg.y_origin);
//     double theta_1_in_degrees = theta_1 * (180.0 / M_PI); //角度
//     float vehicle_yaw = - M_PI * (msg.yaw/180.0); //逆时针为正
//     double theta_2 = theta_1 + vehicle_yaw; //弧度

//     float x_origin = vehicle_x - sqrt(msg.x_origin*msg.x_origin + msg.y_origin*msg.y_origin) * cos(theta_2);
//     float y_origin = vehicle_y - sqrt(msg.x_origin*msg.x_origin + msg.y_origin*msg.y_origin) * sin(theta_2);
//     ROS_INFO("costmap_origin is in world: (x:%f, y:%f)", x_origin, y_origin);
//     point1.x = x_origin;
//     point1.y = y_origin;
//     point1.z = 0.0;
//     points.points.push_back(point1);
//     std_msgs::ColorRGBA color3;
//     color3.r = 0.0f; // 蓝色
//     color3.g = 0.0f;
//     color3.b = 1.0f;
//     color3.a = 1.0f;
//     points.colors.push_back(color3);
//     // 障碍点计算
//     std::vector<std::pair<double, double>> coordinates_obstacle;
//     size_t idx = 0;
//     for (const auto& byte : msg.compressed_data) {
//         for (int i=0; i<8 && idx<msg.original_length; ++i) {
//             bool bit = (byte >> i) & 0x01; // 低位优先解包
//             if(bit)
//             {
//                 int index = idx;
//                 int index_x = index % msg.width;
//                 int index_y = index / msg.width;
//                 ROS_INFO("index is: (index_x:%d, index_y:%d)", index_x, index_y);

//                 // 障碍中心点
//                 double theta_3 = atan(((index_y+0.5)*msg.resolution)/((index_x+0.5)*msg.resolution));
//                 float obstacle_distance = sqrt(pow((index_x+0.5)*msg.resolution, 2) + pow((index_y+0.5)*msg.resolution, 2));
//                 double theta_4 = theta_3 + vehicle_yaw;
//                 float obstacle_x = x_origin + obstacle_distance*cos(theta_4);
//                 float obstacle_y = y_origin + obstacle_distance*sin(theta_4);
//                 ROS_INFO("obstacle is in world: (x:%f, y:%f)", obstacle_x, obstacle_y);
//                 point1.x = obstacle_x;
//                 point1.y = obstacle_y;
//                 point1.z = 0.0;
//                 points.points.push_back(point1);
//                 coordinates_obstacle.push_back({obstacle_x,obstacle_y});
//                 std_msgs::ColorRGBA color5;
//                 color5.r = 0.0f; 
//                 color5.g = 0.5f;
//                 color5.b = 0.5f;
//                 color5.a = 1.0f;
//                 points.colors.push_back(color5);
//             }
//             idx++;
//         }
//     }

//     std::ofstream outfile2;
//     outfile2.open("/home/robot/Desktop/mqtt_test/mqtt_test_hunter/documents/obstacle.txt", std::ios::app);  // 追加模式写入
//     if (outfile2.is_open()) {
//         for (const auto& coord : coordinates_obstacle) {
//             outfile2 << coord.first << " " << coord.second << std::endl; // 每行存储一对坐标
//         }
//         outfile2.close();
//         ROS_INFO(" coordinates_obstacle Data saved to output.txt");
//         ros::shutdown();  // 保存一次后关闭节点
//     } else {
//         ROS_ERROR("Failed to open file obstacle!");
//     }

//     // int index = 100+200*100;
//     // int index_x = index % msg.width;
//     // int index_y = index / msg.width;
//     // ROS_INFO("index is: (index_x:%d, index_y:%d)", index_x, index_y);

//     // // 障碍中心点
//     // double theta_3 = atan(((index_y+0.5)*msg.resolution)/((index_x+0.5)*msg.resolution));
//     // float obstacle_distance = sqrt(pow((index_x+0.5)*msg.resolution, 2) + pow((index_y+0.5)*msg.resolution, 2));
//     // double theta_4 = theta_3 + vehicle_yaw;
//     // float obstacle_x = x_origin + obstacle_distance*cos(theta_4);
//     // float obstacle_y = y_origin + obstacle_distance*sin(theta_4);
//     // ROS_INFO("obstacle is in world: (x:%f, y:%f)", obstacle_x, obstacle_y);
//     // point1.x = obstacle_x;
//     // point1.y = obstacle_y;
//     // point1.z = 0.0;
//     // points.points.push_back(point1);
//     // std_msgs::ColorRGBA color5;
//     // color5.r = 0.0f; 
//     // color5.g = 0.5f;
//     // color5.b = 0.5f;
//     // color5.a = 1.0f;
//     // points.colors.push_back(color5);

//     //计算四角位置
//     std::vector<std::pair<double, double>> coordinates_sijiao;
//     coordinates_sijiao.push_back({x_origin,y_origin});// 0
//     float sijiao_x = x_origin + msg.width*msg.resolution*cos(vehicle_yaw);
//     float sijiao_y = y_origin + msg.width*msg.resolution*sin(vehicle_yaw);
//     coordinates_sijiao.push_back({sijiao_x,sijiao_y});// 1
//     point1.x = sijiao_x;
//     point1.y = sijiao_y;
//     point1.z = 0.0;
//     points.points.push_back(point1);
//     std_msgs::ColorRGBA color4;
//     color4.r = 0.5f; 
//     color4.g = 0.5f;
//     color4.b = 0.0f;
//     color4.a = 1.0f;
//     points.colors.push_back(color4);
//     float sijiao2_dis = sqrt(pow(msg.width*msg.resolution, 2) + pow(msg.height*msg.resolution, 2));
//     sijiao_x = x_origin + sijiao2_dis*cos(atan(msg.height/msg.width)+vehicle_yaw);
//     sijiao_y = y_origin + sijiao2_dis*sin(atan(msg.height/msg.width)+vehicle_yaw);
//     coordinates_sijiao.push_back({sijiao_x,sijiao_y});// 2
//     point1.x = sijiao_x;
//     point1.y = sijiao_y;
//     point1.z = 0.0;
//     points.points.push_back(point1);
//     points.colors.push_back(color4);
//     sijiao_x = x_origin + msg.height*msg.resolution*cos(M_PI/2+vehicle_yaw);
//     sijiao_y = y_origin + msg.height*msg.resolution*sin(M_PI/2+vehicle_yaw);
//     coordinates_sijiao.push_back({sijiao_x,sijiao_y});// 3
//     point1.x = sijiao_x;
//     point1.y = sijiao_y;
//     point1.z = 0.0;
//     points.points.push_back(point1);
//     points.colors.push_back(color4);

//     std::ofstream outfile3;
//     outfile3.open("/home/robot/Desktop/mqtt_test/mqtt_test_hunter/documents/coordinates_sijiao.txt", std::ios::app);  // 追加模式写入
//     if (outfile3.is_open()) {
//         for (const auto& coord : coordinates_sijiao) {
//             outfile3 << coord.first << " " << coord.second << std::endl; // 每行存储一对坐标
//         }
//         outfile3.close();
//         ROS_INFO(" coordinates_sijiao Data saved to output.txt");
//         ros::shutdown();  // 保存一次后关闭节点
//     } else {
//         ROS_ERROR("Failed to open file coordinates_sijiao!");
//     }

//     // 遍历并打印坐标
//     for (const auto& coord : coordinates_sijiao) {
//         std::cout << "(" << coord.first << ", " << coord.second << ")" << std::endl;
//     }
//     marker_pub_.publish(points);

// }




void RemoteControl::run(){
    costmapCompress_sub_ = nh_.subscribe<costmap_compress::CompressedOccupancyGrid>(input_costmapCompress_topic_, 1, &RemoteControl::costmapCompress_callback, this);
    gnssStatus_sub_ = nh_.subscribe<robocoachMsg::GnssStatus>(input_gnssStatus_topic, 1, &RemoteControl::gnssStatus_callback, this);
    carInfo_sub_ = nh_.subscribe<robocoachMsg::CarInfo>(input_carInfo_topic_, 1, &RemoteControl::carInfo_callback, this);
    remoteControl_pub_ = nh_.advertise<remote_control::vehicleToRemote>(output_vehicleToRemote_topic_, 1);
    marker_pub_ = nh_.advertise<visualization_msgs::Marker>("/visualization_marker", 1);
    ros::Timer timer = nh_.createTimer(ros::Duration(0.2), &RemoteControl::process, this, false, true);
    ros::spin();
}